/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

/*
 * cvhaarclassifier.cpp
 *
 * haar classifiers (stump, CART, stage, cascade)
 */

#include "_cvhaartraining.h"


CvIntHaarClassifier* icvCreateCARTHaarClassifier(int count) {
    CvCARTHaarClassifier* cart;
    size_t datasize;

    datasize = sizeof(*cart) +
               (sizeof(int) +
                sizeof(CvTHaarFeature) + sizeof(CvFastHaarFeature) +
                sizeof(float) + sizeof(int) + sizeof(int)) * count +
               sizeof(float) * (count + 1);

    cart = (CvCARTHaarClassifier*) cvAlloc(datasize);
    memset(cart, 0, datasize);

    cart->feature = (CvTHaarFeature*)(cart + 1);
    cart->fastfeature = (CvFastHaarFeature*)(cart->feature + count);
    cart->threshold = (float*)(cart->fastfeature + count);
    cart->left = (int*)(cart->threshold + count);
    cart->right = (int*)(cart->left + count);
    cart->val = (float*)(cart->right + count);
    cart->compidx = (int*)(cart->val + count + 1);
    cart->count = count;
    cart->eval = icvEvalCARTHaarClassifier;
    cart->save = icvSaveCARTHaarClassifier;
    cart->release = icvReleaseHaarClassifier;

    return (CvIntHaarClassifier*) cart;
}


void icvReleaseHaarClassifier(CvIntHaarClassifier** classifier) {
    cvFree(classifier);
    *classifier = NULL;
}


void icvInitCARTHaarClassifier(CvCARTHaarClassifier* carthaar, CvCARTClassifier* cart,
                               CvIntHaarFeatures* intHaarFeatures) {
    int i;

    for (i = 0; i < cart->count; i++) {
        carthaar->feature[i] = intHaarFeatures->feature[cart->compidx[i]];
        carthaar->fastfeature[i] = intHaarFeatures->fastfeature[cart->compidx[i]];
        carthaar->threshold[i] = cart->threshold[i];
        carthaar->left[i] = cart->left[i];
        carthaar->right[i] = cart->right[i];
        carthaar->val[i] = cart->val[i];
        carthaar->compidx[i] = cart->compidx[i];
    }
    carthaar->count = cart->count;
    carthaar->val[cart->count] = cart->val[cart->count];
}


float icvEvalCARTHaarClassifier(CvIntHaarClassifier* classifier,
                                sum_type* sum, sum_type* tilted, float normfactor) {
    int idx = 0;

    do {
        if (cvEvalFastHaarFeature(
                    ((CvCARTHaarClassifier*) classifier)->fastfeature + idx, sum, tilted)
                < (((CvCARTHaarClassifier*) classifier)->threshold[idx] * normfactor)) {
            idx = ((CvCARTHaarClassifier*) classifier)->left[idx];
        } else {
            idx = ((CvCARTHaarClassifier*) classifier)->right[idx];
        }
    } while (idx > 0);

    return ((CvCARTHaarClassifier*) classifier)->val[-idx];
}


CvIntHaarClassifier* icvCreateStageHaarClassifier(int count, float threshold) {
    CvStageHaarClassifier* stage;
    size_t datasize;

    datasize = sizeof(*stage) + sizeof(CvIntHaarClassifier*) * count;
    stage = (CvStageHaarClassifier*) cvAlloc(datasize);
    memset(stage, 0, datasize);

    stage->count = count;
    stage->threshold = threshold;
    stage->classifier = (CvIntHaarClassifier**)(stage + 1);

    stage->eval = icvEvalStageHaarClassifier;
    stage->save = icvSaveStageHaarClassifier;
    stage->release = icvReleaseStageHaarClassifier;

    return (CvIntHaarClassifier*) stage;
}


void icvReleaseStageHaarClassifier(CvIntHaarClassifier** classifier) {
    int i;

    for (i = 0; i < ((CvStageHaarClassifier*) *classifier)->count; i++) {
        if (((CvStageHaarClassifier*) *classifier)->classifier[i] != NULL) {
            ((CvStageHaarClassifier*) *classifier)->classifier[i]->release(
                &(((CvStageHaarClassifier*) *classifier)->classifier[i]));
        }
    }

    cvFree(classifier);
    *classifier = NULL;
}


float icvEvalStageHaarClassifier(CvIntHaarClassifier* classifier,
                                 sum_type* sum, sum_type* tilted, float normfactor) {
    int i;
    float stage_sum;

    stage_sum = 0.0F;
    for (i = 0; i < ((CvStageHaarClassifier*) classifier)->count; i++) {
        stage_sum +=
            ((CvStageHaarClassifier*) classifier)->classifier[i]->eval(
                ((CvStageHaarClassifier*) classifier)->classifier[i],
                sum, tilted, normfactor);
    }

    return stage_sum;
}


CvIntHaarClassifier* icvCreateCascadeHaarClassifier(int count) {
    CvCascadeHaarClassifier* ptr;
    size_t datasize;

    datasize = sizeof(*ptr) + sizeof(CvIntHaarClassifier*) * count;
    ptr = (CvCascadeHaarClassifier*) cvAlloc(datasize);
    memset(ptr, 0, datasize);

    ptr->count = count;
    ptr->classifier = (CvIntHaarClassifier**)(ptr + 1);

    ptr->eval = icvEvalCascadeHaarClassifier;
    ptr->save = NULL;
    ptr->release = icvReleaseCascadeHaarClassifier;

    return (CvIntHaarClassifier*) ptr;
}


void icvReleaseCascadeHaarClassifier(CvIntHaarClassifier** classifier) {
    int i;

    for (i = 0; i < ((CvCascadeHaarClassifier*) *classifier)->count; i++) {
        if (((CvCascadeHaarClassifier*) *classifier)->classifier[i] != NULL) {
            ((CvCascadeHaarClassifier*) *classifier)->classifier[i]->release(
                &(((CvCascadeHaarClassifier*) *classifier)->classifier[i]));
        }
    }

    cvFree(classifier);
    *classifier = NULL;
}


float icvEvalCascadeHaarClassifier(CvIntHaarClassifier* classifier,
                                   sum_type* sum, sum_type* tilted, float normfactor) {
    int i;

    for (i = 0; i < ((CvCascadeHaarClassifier*) classifier)->count; i++) {
        if (((CvCascadeHaarClassifier*) classifier)->classifier[i]->eval(
                    ((CvCascadeHaarClassifier*) classifier)->classifier[i],
                    sum, tilted, normfactor)
                < (((CvStageHaarClassifier*)
                    ((CvCascadeHaarClassifier*) classifier)->classifier[i])->threshold
                   - CV_THRESHOLD_EPS)) {
            return 0.0;
        }
    }

    return 1.0;
}


void icvSaveHaarFeature(CvTHaarFeature* feature, FILE* file) {
    fprintf(file, "%d\n", ((feature->rect[2].weight == 0.0F) ? 2 : 3));
    fprintf(file, "%d %d %d %d %d %d\n",
            feature->rect[0].r.x,
            feature->rect[0].r.y,
            feature->rect[0].r.width,
            feature->rect[0].r.height,
            0,
            (int)(feature->rect[0].weight));
    fprintf(file, "%d %d %d %d %d %d\n",
            feature->rect[1].r.x,
            feature->rect[1].r.y,
            feature->rect[1].r.width,
            feature->rect[1].r.height,
            0,
            (int)(feature->rect[1].weight));
    if (feature->rect[2].weight != 0.0F) {
        fprintf(file, "%d %d %d %d %d %d\n",
                feature->rect[2].r.x,
                feature->rect[2].r.y,
                feature->rect[2].r.width,
                feature->rect[2].r.height,
                0,
                (int)(feature->rect[2].weight));
    }
    fprintf(file, "%s\n", &(feature->desc[0]));
}


void icvLoadHaarFeature(CvTHaarFeature* feature, FILE* file) {
    int nrect;
    int j;
    int tmp;
    int weight;

    nrect = 0;
    fscanf(file, "%d", &nrect);

    assert(nrect <= CV_HAAR_FEATURE_MAX);

    for (j = 0; j < nrect; j++) {
        fscanf(file, "%d %d %d %d %d %d",
               &(feature->rect[j].r.x),
               &(feature->rect[j].r.y),
               &(feature->rect[j].r.width),
               &(feature->rect[j].r.height),
               &tmp, &weight);
        feature->rect[j].weight = (float) weight;
    }
    for (j = nrect; j < CV_HAAR_FEATURE_MAX; j++) {
        feature->rect[j].r.x = 0;
        feature->rect[j].r.y = 0;
        feature->rect[j].r.width = 0;
        feature->rect[j].r.height = 0;
        feature->rect[j].weight = 0.0f;
    }
    fscanf(file, "%s", &(feature->desc[0]));
    feature->tilted = (feature->desc[0] == 't');
}


void icvSaveCARTHaarClassifier(CvIntHaarClassifier* classifier, FILE* file) {
    int i;
    int count;

    count = ((CvCARTHaarClassifier*) classifier)->count;
    fprintf(file, "%d\n", count);
    for (i = 0; i < count; i++) {
        icvSaveHaarFeature(&(((CvCARTHaarClassifier*) classifier)->feature[i]), file);
        fprintf(file, "%e %d %d\n",
                ((CvCARTHaarClassifier*) classifier)->threshold[i],
                ((CvCARTHaarClassifier*) classifier)->left[i],
                ((CvCARTHaarClassifier*) classifier)->right[i]);
    }
    for (i = 0; i <= count; i++) {
        fprintf(file, "%e ", ((CvCARTHaarClassifier*) classifier)->val[i]);
    }
    fprintf(file, "\n");
}


CvIntHaarClassifier* icvLoadCARTHaarClassifier(FILE* file, int step) {
    CvCARTHaarClassifier* ptr;
    int i;
    int count;

    ptr = NULL;
    fscanf(file, "%d", &count);
    if (count > 0) {
        ptr = (CvCARTHaarClassifier*) icvCreateCARTHaarClassifier(count);
        for (i = 0; i < count; i++) {
            icvLoadHaarFeature(&(ptr->feature[i]), file);
            fscanf(file, "%f %d %d", &(ptr->threshold[i]), &(ptr->left[i]),
                   &(ptr->right[i]));
        }
        for (i = 0; i <= count; i++) {
            fscanf(file, "%f", &(ptr->val[i]));
        }
        icvConvertToFastHaarFeature(ptr->feature, ptr->fastfeature, ptr->count, step);
    }

    return (CvIntHaarClassifier*) ptr;
}


void icvSaveStageHaarClassifier(CvIntHaarClassifier* classifier, FILE* file) {
    int count;
    int i;
    float threshold;

    count = ((CvStageHaarClassifier*) classifier)->count;
    fprintf(file, "%d\n", count);
    for (i = 0; i < count; i++) {
        ((CvStageHaarClassifier*) classifier)->classifier[i]->save(
            ((CvStageHaarClassifier*) classifier)->classifier[i], file);
    }

    threshold = ((CvStageHaarClassifier*) classifier)->threshold;

    /* to be compatible with the previous implementation */
    /* threshold = 2.0F * ((CvStageHaarClassifier*) classifier)->threshold - count; */

    fprintf(file, "%e\n", threshold);
}



CvIntHaarClassifier* icvLoadCARTStageHaarClassifierF(FILE* file, int step) {
    CvStageHaarClassifier* ptr = NULL;

    //CV_FUNCNAME( "icvLoadCARTStageHaarClassifierF" );

    __BEGIN__;

    if (file != NULL) {
        int count;
        int i;
        float threshold;

        count = 0;
        fscanf(file, "%d", &count);
        if (count > 0) {
            ptr = (CvStageHaarClassifier*) icvCreateStageHaarClassifier(count, 0.0F);
            for (i = 0; i < count; i++) {
                ptr->classifier[i] = icvLoadCARTHaarClassifier(file, step);
            }

            fscanf(file, "%f", &threshold);

            ptr->threshold = threshold;
            /* to be compatible with the previous implementation */
            /* ptr->threshold = 0.5F * (threshold + count); */
        }
        if (feof(file)) {
            ptr->release((CvIntHaarClassifier**) &ptr);
            ptr = NULL;
        }
    }

    __END__;

    return (CvIntHaarClassifier*) ptr;
}


CvIntHaarClassifier* icvLoadCARTStageHaarClassifier(const char* filename, int step) {
    CvIntHaarClassifier* ptr = NULL;

    CV_FUNCNAME("icvLoadCARTStageHaarClassifier");

    __BEGIN__;

    FILE* file;

    file = fopen(filename, "r");
    if (file) {
        CV_CALL(ptr = icvLoadCARTStageHaarClassifierF(file, step));
        fclose(file);
    }

    __END__;

    return ptr;
}

/* tree cascade classifier */

/* evaluates a tree cascade classifier */

float icvEvalTreeCascadeClassifier(CvIntHaarClassifier* classifier,
                                   sum_type* sum, sum_type* tilted, float normfactor) {
    CvTreeCascadeNode* ptr;

    ptr = ((CvTreeCascadeClassifier*) classifier)->root;

    while (ptr) {
        if (ptr->stage->eval((CvIntHaarClassifier*) ptr->stage,
                             sum, tilted, normfactor)
                >= ptr->stage->threshold - CV_THRESHOLD_EPS) {
            ptr = ptr->child;
        } else {
            while (ptr && ptr->next == NULL) { ptr = ptr->parent; }
            if (ptr == NULL) { return 0.0F; }
            ptr = ptr->next;
        }
    }

    return 1.0F;
}

/* sets path int the tree form the root to the leaf node */

void icvSetLeafNode(CvTreeCascadeClassifier* tcc, CvTreeCascadeNode* leaf) {
    CV_FUNCNAME("icvSetLeafNode");

    __BEGIN__;

    CvTreeCascadeNode* ptr;

    ptr = NULL;
    while (leaf) {
        leaf->child_eval = ptr;
        ptr = leaf;
        leaf = leaf->parent;
    }

    leaf = tcc->root;
    while (leaf && leaf != ptr) { leaf = leaf->next; }
    if (leaf != ptr) {
        CV_ERROR(CV_StsError, "Invalid tcc or leaf node.");
    }

    tcc->root_eval = ptr;

    __END__;
}

/* evaluates a tree cascade classifier. used in filtering */

float icvEvalTreeCascadeClassifierFilter(CvIntHaarClassifier* classifier, sum_type* sum,
        sum_type* tilted, float normfactor) {
    CvTreeCascadeNode* ptr;
    CvTreeCascadeClassifier* tree;

    tree = (CvTreeCascadeClassifier*) classifier;



    ptr = ((CvTreeCascadeClassifier*) classifier)->root_eval;
    while (ptr) {
        if (ptr->stage->eval((CvIntHaarClassifier*) ptr->stage,
                             sum, tilted, normfactor)
                < ptr->stage->threshold - CV_THRESHOLD_EPS) {
            return 0.0F;
        }
        ptr = ptr->child_eval;
    }

    return 1.0F;
}

/* creates tree cascade node */

CvTreeCascadeNode* icvCreateTreeCascadeNode() {
    CvTreeCascadeNode* ptr = NULL;

    CV_FUNCNAME("icvCreateTreeCascadeNode");

    __BEGIN__;
    size_t data_size;

    data_size = sizeof(*ptr);
    CV_CALL(ptr = (CvTreeCascadeNode*) cvAlloc(data_size));
    memset(ptr, 0, data_size);

    __END__;

    return ptr;
}

/* releases all tree cascade nodes accessible via links */

void icvReleaseTreeCascadeNodes(CvTreeCascadeNode** node) {
    //CV_FUNCNAME( "icvReleaseTreeCascadeNodes" );

    __BEGIN__;

    if (node && *node) {
        CvTreeCascadeNode* ptr;
        CvTreeCascadeNode* ptr_;

        ptr = *node;

        while (ptr) {
            while (ptr->child) { ptr = ptr->child; }

            if (ptr->stage) { ptr->stage->release((CvIntHaarClassifier**) &ptr->stage); }
            ptr_ = ptr;

            while (ptr && ptr->next == NULL) { ptr = ptr->parent; }
            if (ptr) { ptr = ptr->next; }

            cvFree(&ptr_);
        }
    }

    __END__;
}


/* releases tree cascade classifier */

void icvReleaseTreeCascadeClassifier(CvIntHaarClassifier** classifier) {
    if (classifier && *classifier) {
        icvReleaseTreeCascadeNodes(&((CvTreeCascadeClassifier*) *classifier)->root);
        cvFree(classifier);
        *classifier = NULL;
    }
}


void icvPrintTreeCascade(CvTreeCascadeNode* root) {
    //CV_FUNCNAME( "icvPrintTreeCascade" );

    __BEGIN__;

    CvTreeCascadeNode* node;
    CvTreeCascadeNode* n;
    char buf0[256];
    char buf[256];
    int level;
    int i;
    int max_level;

    node = root;
    level = max_level = 0;
    while (node) {
        while (node->child) { node = node->child; level++; }
        if (level > max_level) { max_level = level; }
        while (node && !node->next) { node = node->parent; level--; }
        if (node) { node = node->next; }
    }

    printf("\nTree Classifier\n");
    printf("Stage\n");
    for (i = 0; i <= max_level; i++) { printf("+---"); }
    printf("+\n");
    for (i = 0; i <= max_level; i++) { printf("|%3d", i); }
    printf("|\n");
    for (i = 0; i <= max_level; i++) { printf("+---"); }
    printf("+\n\n");

    node = root;

    buf[0] = 0;
    while (node) {
        sprintf(buf + strlen(buf), "%3d", node->idx);
        while (node->child) {
            node = node->child;
            sprintf(buf + strlen(buf),
                    ((node->idx < 10) ? "---%d" : ((node->idx < 100) ? "--%d" : "-%d")),
                    node->idx);
        }
        printf(" %s\n", buf);

        while (node && !node->next) { node = node->parent; }
        if (node) {
            node = node->next;

            n = node->parent;
            buf[0] = 0;
            while (n) {
                if (n->next) {
                    sprintf(buf0, "  | %s", buf);
                } else {
                    sprintf(buf0, "    %s", buf);
                }
                strcpy(buf, buf0);
                n = n->parent;
            }
            printf(" %s  |\n", buf);
        }
    }
    printf("\n");
    fflush(stdout);

    __END__;
}



CvIntHaarClassifier* icvLoadTreeCascadeClassifier(const char* filename, int step,
        int* splits) {
    CvTreeCascadeClassifier* ptr = NULL;
    CvTreeCascadeNode** nodes = NULL;

    CV_FUNCNAME("icvLoadTreeCascadeClassifier");

    __BEGIN__;

    size_t data_size;
    CvStageHaarClassifier* stage;
    char stage_name[PATH_MAX];
    char* suffix;
    int i, num;
    FILE* f;
    int result, parent = 0, next = 0;
    int stub;

    if (!splits) { splits = &stub; }

    *splits = 0;

    data_size = sizeof(*ptr);

    CV_CALL(ptr = (CvTreeCascadeClassifier*) cvAlloc(data_size));
    memset(ptr, 0, data_size);

    ptr->eval = icvEvalTreeCascadeClassifier;
    ptr->release = icvReleaseTreeCascadeClassifier;

    sprintf(stage_name, "%s/", filename);
    suffix = stage_name + strlen(stage_name);

    for (i = 0; ; i++) {
        sprintf(suffix, "%d/%s", i, CV_STAGE_CART_FILE_NAME);
        f = fopen(stage_name, "r");
        if (!f) { break; }
        fclose(f);
    }
    num = i;

    if (num < 1) { EXIT; }

    data_size = sizeof(*nodes) * num;
    CV_CALL(nodes = (CvTreeCascadeNode**) cvAlloc(data_size));

    for (i = 0; i < num; i++) {
        sprintf(suffix, "%d/%s", i, CV_STAGE_CART_FILE_NAME);
        f = fopen(stage_name, "r");
        CV_CALL(stage = (CvStageHaarClassifier*)
                        icvLoadCARTStageHaarClassifierF(f, step));

        result = (f && stage) ? fscanf(f, "%d%d", &parent, &next) : 0;
        if (f) { fclose(f); }

        if (result != 2) {
            num = i;
            break;
        }

        printf("Stage %d loaded\n", i);

        if (parent >= i || (next != -1 && next != i + 1)) {
            CV_ERROR(CV_StsError, "Invalid tree links");
        }

        CV_CALL(nodes[i] = icvCreateTreeCascadeNode());
        nodes[i]->stage = stage;
        nodes[i]->idx = i;
        nodes[i]->parent = (parent != -1) ? nodes[parent] : NULL;
        nodes[i]->next = (next != -1) ? nodes[i] : NULL;
        nodes[i]->child = NULL;
    }
    for (i = 0; i < num; i++) {
        if (nodes[i]->next) {
            (*splits)++;
            nodes[i]->next = nodes[i + 1];
        }
        if (nodes[i]->parent && nodes[i]->parent->child == NULL) {
            nodes[i]->parent->child = nodes[i];
        }
    }
    ptr->root = nodes[0];
    ptr->next_idx = num;

    __END__;

    cvFree(&nodes);

    return (CvIntHaarClassifier*) ptr;
}


CvTreeCascadeNode* icvFindDeepestLeaves(CvTreeCascadeClassifier* tcc) {
    CvTreeCascadeNode* leaves;

    //CV_FUNCNAME( "icvFindDeepestLeaves" );

    __BEGIN__;

    int level, cur_level;
    CvTreeCascadeNode* ptr;
    CvTreeCascadeNode* last;

    leaves = last = NULL;

    ptr = tcc->root;
    level = -1;
    cur_level = 0;

    /* find leaves with maximal level */
    while (ptr) {
        if (ptr->child) { ptr = ptr->child; cur_level++; }
        else {
            if (cur_level == level) {
                last->next_same_level = ptr;
                ptr->next_same_level = NULL;
                last = ptr;
            }
            if (cur_level > level) {
                level = cur_level;
                leaves = last = ptr;
                ptr->next_same_level = NULL;
            }
            while (ptr && ptr->next == NULL) { ptr = ptr->parent; cur_level--; }
            if (ptr) { ptr = ptr->next; }
        }
    }

    __END__;

    return leaves;
}

/* End of file. */
